#include "Temp.h"
#include "ObjHandler.h"

Cloth::Cloth(void)
{
}

Cloth::~Cloth(void)
{
}

void Cloth::InitCloth( char* filename )
{
	bu = bv = 5.0;
	k_stretch = 1;
	k_shear = 0.4;
	d_stretch = 2;
	d_shear = 0.002;

	OBJHandler mesh;
	mesh.LoadOBJascii(filename);

	for (int i=0;i<mesh.numVerts;i++)
	{
		Vertex* v=new Vertex;

		v->position=vec3f(mesh.verts[i].x,mesh.verts[i].y,mesh.verts[i].z);
		v->uv=vec2f(mesh.uvs[i].u,mesh.uvs[i].v);
		v->velocity=vec3f(0,0,0);
		v->force=vec3f(0,0,0);
		v->mass=0.0;
		v->index=i;

		vertices.push_back(v);
	}

	bool edge_dup;

	for (int i=0;i<mesh.numTris;i++)
	{
		Triangle* tri=new Triangle;

		tri->vertices[0]=vertices.at(mesh.tris[i].v1);
		tri->vertices[1]=vertices.at(mesh.tris[i].v2);
		tri->vertices[2]=vertices.at(mesh.tris[i].v3);

		tri->setArea();

		tri->vertices[0]->mass += 0.333 * DENSITY * tri->area;
		tri->vertices[1]->mass += 0.333 * DENSITY * tri->area;
		tri->vertices[2]->mass += 0.333 * DENSITY * tri->area;

		vec2f uv0, uv1, uv2;

		uv0 = tri->vertices[0]->uv;
		uv1 = tri->vertices[1]->uv;
		uv2 = tri->vertices[2]->uv;

		mat2f Muv;
		Muv[0][0] = uv1[0] - uv0[0];
		Muv[0][1] = uv2[0] - uv0[0];
		Muv[1][0] = uv1[1] - uv0[1];
		Muv[1][1] = uv2[1] - uv0[1];

		tri->uvInv = Muv.Inverse();

		/*float det = uv2[0] * uv0[1] + uv0[0] * uv1[1] + uv1[0] * uv2[1] - uv1[0] * uv0[1] - uv2[0] * uv1[1] - uv0[0] * uv2[1];

		tri->uvInv[0][0] = ( uv2[1] - uv0[1] ) / det;
		tri->uvInv[0][1] = ( uv0[0] - uv2[0] ) / det;
		tri->uvInv[1][0] = ( uv0[1] - uv1[1] ) / det;
		tri->uvInv[1][1] = ( uv1[0] - uv0[0] ) / det;*/

		/************************************************************************/
		/*							Edge List Init                              */
		/************************************************************************/

		//	e01

		edge_dup=false;

		for (int j=0;j<edges.size();j++)
		{
			Edge* e=edges.at(j);

			if ((tri->vertices[0]==e->vertices[0]&&tri->vertices[1]==e->vertices[1])||(tri->vertices[0]==e->vertices[1]&&tri->vertices[1]==e->vertices[0]))
			{
				edge_dup=true;

				e->boarder=1;
				e->triangles[1]=tri;

				tri->edges[0]=e;

				break;
			}
		}

		if (!edge_dup)
		{
			Edge* edge=new Edge;

			edge->vertices[0]=tri->vertices[0];
			edge->vertices[1]=tri->vertices[1];

			edge->boarder=0;
			edge->triangles[0]=tri;
			
			edges.push_back(edge);

			tri->edges[0]=edge;
		}

		//	e12

		edge_dup=false;

		for (int j=0;j<edges.size();j++)
		{
			Edge* e=edges.at(j);

			if ((tri->vertices[1]==e->vertices[0]&&tri->vertices[2]==e->vertices[1])||(tri->vertices[2]==e->vertices[0]&&tri->vertices[1]==e->vertices[1]))
			{
				edge_dup=true;

				e->boarder=1;
				e->triangles[1]=tri;

				tri->edges[1]=e;

				break;
			}
		}

		if (!edge_dup)
		{
			Edge* edge=new Edge;

			edge->vertices[0]=tri->vertices[1];
			edge->vertices[1]=tri->vertices[2];

			edge->boarder=0;
			edge->triangles[0]=tri;

			edges.push_back(edge);

			tri->edges[1]=edge;
		}

		//	e20

		edge_dup=false;

		for (int j=0;j<edges.size();j++)
		{
			Edge* e=edges.at(j);

			if ((tri->vertices[0]==e->vertices[0]&&tri->vertices[2]==e->vertices[1])||(tri->vertices[0]==e->vertices[1]&&tri->vertices[2]==e->vertices[0]))
			{
				edge_dup=true;

				e->boarder=1;
				e->triangles[1]=tri;

				tri->edges[2]=e;

				break;
			}
		}

		if (!edge_dup)
		{
			Edge* edge=new Edge;

			edge->vertices[0]=tri->vertices[2];
			edge->vertices[1]=tri->vertices[0];

			edge->boarder=0;
			edge->triangles[0]=tri;

			edges.push_back(edge);

			tri->edges[2]=edge;
		}

		/************************************************************************/
		/*								End                                     */
		/************************************************************************/

		triangles.push_back(tri);
	}

	// Stiffness Matrix Init

	solver.InitSparseStiffness(mesh.numVerts);

	for (int i=0;i<edges.size();i++)
	{
		Edge* edge=edges.at(i);

		Vertex *va, *vb;

		va = edge->vertices[0];
		vb = edge->vertices[1];

		// for vertex a

		StiffChain* sc = solver.sparse_M[va->index];

		if (sc->size()==0)
		{
			Stiff* stiffness = new Stiff;
			stiffness->index[0]=va->index;
			stiffness->index[1]=va->index;

			sc->push_back(stiffness);
		}

		Stiff* stiffness_a = new Stiff;
		stiffness_a->index[0]=va->index;
		stiffness_a->index[1]=vb->index;

		sc->push_back(stiffness_a);

		// for vertex b

		sc = solver.sparse_M[vb->index];

		if (sc->size()==0)
		{
			Stiff* stiffness = new Stiff;
			stiffness->index[0]=vb->index;
			stiffness->index[1]=vb->index;

			sc->push_back(stiffness);
		}

		Stiff* stiffness_b = new Stiff;
		stiffness_b->index[0]=vb->index;
		stiffness_b->index[1]=va->index;

		sc->push_back(stiffness_b);

	}

}

void Cloth::UpdateCloth( float dt )
{
	timestep=dt;

	UpdateForce();
	UpdateStiffness();
	TimeIntegration();
	ConstrainFixedPoint();
	
}

void Cloth::UpdateForce()
{
	ApplyExternalForce();
	ApplyStretchShearForce();
	ApplyBendForce();
}

void Cloth::ApplyExternalForce()
{
	for (int i=0;i<vertices.size();i++)
	{
		vertices.at(i)->force=vec3f(0.0,-0.98,0.0) * vertices.at(i)->mass;
	}
}

void Cloth::UpdateStiffness()
{
	ResetStiffness();
	AssemblyStiffness();
	//AssemblyTarget();
	AssemblyCoefficient();
}

void Cloth::TimeIntegration()
{
	//ForwardEuler();
	BackwardEuler();
}

void Cloth::ForwardEuler()
{
	for (int i=0;i<vertices.size();i++)
	{
		Vertex* v=vertices.at(i);

		v->velocity += timestep * v->force / v->mass;
		v->position += timestep * v->velocity;
	}
}

void Cloth::ConstrainFixedPoint()
{
	vertices.at(2)->position -= timestep * vertices.at(2)->velocity;
	vertices.at(1)->position -= timestep * vertices.at(1)->velocity;
}

void Cloth::ApplyStretchShearForce()
{
	vec3f wu, wv;
	vec3f dp1, dp2;
	mat3f wu_p0, wu_p1, wu_p2;
	mat3f wv_p0, wv_p1, wv_p2;
	float wu_n2, wv_n2;
	

	for (int i=0;i<triangles.size();i++)
	{
		Triangle* tri=triangles.at(i);

		Vertex* ca = tri->vertices[0];
		Vertex* cb = tri->vertices[1];
		Vertex* cc = tri->vertices[2];

		// delta_pi and delta_p2

		dp1 = tri->vertices[1]->position - tri->vertices[0]->position;
		dp2 = tri->vertices[2]->position - tri->vertices[0]->position;

		// wu and wv

		tri->wu = wu = dp1 * tri->uvInv[0][0] + dp2 * tri->uvInv[1][0];
		tri->wv = wv = dp1 * tri->uvInv[0][1] + dp2 * tri->uvInv[1][1];

		tri->wu_n2 = wu_n2 = wu.Length();
		tri->wv_n2 = wv_n2 = wv.Length();

		// derivative of wu and wv

		wu_p0 = mat3f::Identity();
		wu_p1 = mat3f::Identity();
		wu_p2 = mat3f::Identity();

		tri->wu_p[0] = wu_p0 *= - tri->uvInv[0][0] - tri->uvInv[1][0];
		tri->wu_p[1] = wu_p1 *= tri->uvInv[0][0];
		tri->wu_p[2] = wu_p2 *= tri->uvInv[1][0];

		wv_p0 = mat3f::Identity();
		wv_p1 = mat3f::Identity();
		wv_p2 = mat3f::Identity();

		tri->wv_p[0] = wv_p0 *= - tri->uvInv[0][1] - tri->uvInv[1][1];
		tri->wv_p[1] = wv_p1 *= tri->uvInv[0][1];
		tri->wv_p[2] = wv_p2 *= tri->uvInv[1][1];

		{	// stretch force

			vec3f m1, m2;
			float c1, c2;
			vec3f inf;

			// force for vertex 0

			m1 = wu_p0 * wu;
			m1 *= (tri->area / wu_n2);

			m2 = wv_p0 * wv;
			m2 *= (tri->area / wv_n2);

			c1 = k_stretch * (tri->area * (wu_n2 - bu));
			c2 = k_stretch * (tri->area * (wv_n2 - bv));

			//c1 += d_stretch * (tri->area * (wu_n2 - bu)) * (m1 * tri->vertices[0]->velocity);
			//c2 += d_stretch * (tri->area * (wv_n2 - bv)) * (m2 * tri->vertices[0]->velocity);

			inf = m1 * c1 + m2 * c2;
			inf *= -1;

			tri->vertices[0]->force += inf;

			// force for vertex 1

			m1 = wu_p1 * wu;
			m1 *= (tri->area / wu_n2);

			m2 = wv_p1 * wv;
			m2 *= (tri->area / wv_n2);

			c1 = k_stretch * (tri->area * (wu_n2 - bu));
			c2 = k_stretch * (tri->area * (wv_n2 - bv));

			//c1 += d_stretch * (tri->area * (wu_n2 - bu)) * (m1 * tri->vertices[1]->velocity);
			//c2 += d_stretch * (tri->area * (wv_n2 - bv)) * (m2 * tri->vertices[1]->velocity);

			inf = m1 * c1 + m2 * c2;
			inf *= -1;

			tri->vertices[1]->force += inf;

			// force for vertex 2

			m1 = wu_p2 * wu;
			m1 *= (tri->area / wu_n2);

			m2 = wv_p2 * wv;
			m2 *= (tri->area / wv_n2);

			c1 = k_stretch * (tri->area * (wu_n2 - bu));
			c2 = k_stretch * (tri->area * (wv_n2 - bv));

			//c1 += d_stretch * (tri->area * (wu_n2 - bu)) * (m1 * tri->vertices[2]->velocity);
			//c2 += d_stretch * (tri->area * (wv_n2 - bv)) * (m2 * tri->vertices[2]->velocity);

			inf = m1 * c1 + m2 * c2;
			inf *= -1;

			tri->vertices[2]->force += inf;

		}
		
	}

}

void Cloth::ApplyBendForce()
{

}

void Cloth::ResetStiffness()
{
	for (int i=0;i<solver.scale;i++)
	{
		StiffChain* sc = solver.sparse_M[i];

		for (int j=0;j<sc->size();j++)
		{
			Stiff* sm = sc->at(j);

			sm->stiff.SetAll(0.0);
		}	
	}

}

mat3f Cloth::OuterProduct( mat3f a, mat3f b )
{
	mat3f result;

	result = a * b.Transpose();

	return result;
}

mat3f Cloth::OuterProduct( vec3f a, vec3f b )
{
	mat3f result;

	for (int i=0;i<3;i++)
	{
		for (int j=0;j<3;j++)
		{
			result[i][j] = a[i] * b[j];
		}
	}

	return result;
}


void Cloth::AssemblyCoefficient()
{

	float Stiff_K[TEMPSCALE][TEMPSCALE];
	float v0[TEMPSCALE];
	float f0[TEMPSCALE];

	for (int i=0;i<vertices.size();i++)
	{
		Vertex* vert = vertices.at(i);

		for (int j=0;j<3;j++)
		{
			for (int k=0;k<vertices.size()*3;k++)
			{
				solver.check_A[i*3+j][k]=0;

				Stiff_K[i*3+j][k]=0;

				if ((i*3+j)==k)
				{
					solver.check_A[i*3+j][k]=vert->mass;
				}
			}
		}
	}

	/*for (int i=0;i<TEMPSCALE;i++)
	{
		for (int j=0;j<TEMPSCALE;j++)
		{
			printf("%.02f ",solver.check_A[i][j]);
		}
		printf("\n");
	}

	printf("----------\n");*/

	for (int i=0;i<vertices.size();i++)
	{
		StiffChain* sc = solver.sparse_M[i];

		for (int j=0;j<sc->size();j++)
		{
			Stiff* sm = sc->at(j);

			int r, c;

			r = sm->index[0] * 3;
			c = sm->index[1] * 3;

			for (int x=0;x<3;x++)
			{
				for (int y=0;y<3;y++)
				{
					solver.check_A[r+x][c+y] -= pow(timestep, 2) * sm->stiff[x][y];
					Stiff_K[r+x][c+y] = sm->stiff[x][y] * timestep;
				}
			}

		}

		Vertex* vert = vertices.at(i);

		v0[i*3+0] = vert->velocity[0];
		v0[i*3+1] = vert->velocity[1];
		v0[i*3+2] = vert->velocity[2];

		f0[i*3+0] = vert->force[0];
		f0[i*3+1] = vert->force[1];
		f0[i*3+2] = vert->force[2];
	}

	static int frame=0;

	frame++;

	/*if (frame>5)
	{
		for (int i=0;i<vertices.size();i++)
		{
			Vertex* vert=vertices.at(i);

			printf("%f %f %f\n",f0[i*3+0]-vert->force[0],f0[i*3+1]-vert->force[1],f0[i*3+2]-vert->force[2]);
		}
	}*/

	/*if (frame>50)
	{
		for (int i=0;i<vertices.size();i++)
		{
			StiffChain* sc=solver.sparse_M[i];

			printf("@  %d\n",i);

			for (int j=0;j<sc->size();j++)
			{
				Stiff* sm=sc->at(j);

				printf("  --  %d %d\n",sm->index[0],sm->index[1]);

				for (int m=0;m<3;m++)
				{
					for (int n=0;n<3;n++)
					{
						printf("%.02f ",sm->stiff[m][n]*timestep);
					}
					printf("\n");
				}
			}
		}

		printf("----------\n\n\n");

		for (int i=0;i<TEMPSCALE;i++)
		{
			for (int j=0;j<TEMPSCALE;j++)
			{
				printf("%.02f ",Stiff_K[i][j]);
			}
			printf("\n");
		}

		printf("----------\n");

	}*/

	


	for (int i=0;i<vertices.size()*3;i++)
	{
		float sum=0;

		for (int j=0;j<vertices.size()*3;j++)
		{
			sum += Stiff_K[i][j] * v0[j];
		}

		solver.check_b[i] = (f0[i] + sum) * timestep;
	}

}

void Cloth::BackwardEuler()
{
	solver.SparseSolver();

	for (int i=0;i<vertices.size();i++)
	{
		Vertex* v=vertices.at(i);

		if (i>60)
		{
			int help;
			help=20;
			help++;
		}

		vec3f delta;

		delta[0] = solver.check_x[i*3+0];
		delta[1] = solver.check_x[i*3+1];
		delta[2] = solver.check_x[i*3+2];

		v->velocity += delta;
		v->position += timestep * v->velocity;
	}
}

void Cloth::AssemblyStiffness()
{

	for (int t=0;t<triangles.size();t++)
	{
		Triangle* tri = triangles.at(t);

		mat3f KC[3][3];

		for (int i=0;i<3;i++)
		{
			for (int j=0;j<3;j++)
			{

				// C_pipj

				mat3f cu1, cv1, cu2, cv2;
				mat3f hessian_cu, hessian_cv;
				mat3f K;
				float cou, cov;

				cu1 = OuterProduct(tri->wu_p[j], tri->wu_p[i]);
				cu1 *= (tri->area / tri->wu_n2);

				cv1 = OuterProduct(tri->wv_p[j], tri->wv_p[i]);
				cv1 *= (tri->area / tri->wv_n2);

				for (int x=0;x<3;x++)
				{
					for (int y=0;y<3;y++)
					{
						vec3f ri, rj;

						switch (x)
						{
						case 0:
							rj = tri->wu_p[j].r0;
							break;

						case 1:
							rj = tri->wu_p[j].r1;
							break;

						case 2:
							rj = tri->wu_p[j].r2;
							break;
						}

						switch (y)
						{
						case 0:
							ri = tri->wu_p[i].r0;
							break;

						case 1:
							ri = tri->wu_p[i].r1;
							break;

						case 2:
							ri = tri->wu_p[i].r2;
							break;
						}

						cu2[x][y] = (ri * tri->wu) * (rj * tri->wu) * (-1 * tri->area / pow(tri->wu_n2, 3));
					}
				}

				for (int x=0;x<3;x++)
				{
					for (int y=0;y<3;y++)
					{
						vec3f ri, rj;

						switch (x)
						{
						case 0:
							rj = tri->wv_p[j].r0;
							break;

						case 1:
							rj = tri->wv_p[j].r1;
							break;

						case 2:
							rj = tri->wv_p[j].r2;
							break;
						}

						switch (y)
						{
						case 0:
							ri = tri->wv_p[i].r0;
							break;

						case 1:
							ri = tri->wv_p[i].r1;
							break;

						case 2:
							ri = tri->wv_p[i].r2;
							break;
						}

						cv2[x][y] = (ri * tri->wv) * (rj * tri->wv) * (-1 * tri->area / pow(tri->wv_n2, 3));
					}
				}

				hessian_cu = cu1 + cu2;
				hessian_cv = cv1 + cv2;

				cou = (tri->area * (tri->wu_n2 - bu));
				cov = (tri->area * (tri->wv_n2 - bv));

				K.SetAll(0.0);

				K = hessian_cu * cou + hessian_cv * cov;

				mat3f firstForU = hessian_cu * cou;



				// Second Term

				vec3f mui, mvi, muj, mvj;

				mui = tri->wu_p[i] * tri->wu;
				mui *= (tri->area / tri->wu_n2);

				mvi = tri->wv_p[i] * tri->wv;
				mvi *= (tri->area / tri->wv_n2);

				muj = tri->wu_p[j] * tri->wu;
				muj *= (tri->area / tri->wu_n2);

				mvj = tri->wv_p[j] * tri->wv;
				mvj *= (tri->area / tri->wv_n2);

				K += (OuterProduct(muj, mui) + OuterProduct(mvj, mvi));

				K *= (-1 * k_stretch);

				KC[i][j] = K;

				Vertex* vert = tri->vertices[i];
				Vertex* neigh = tri->vertices[j];

				StiffChain* sc = solver.sparse_M[vert->index];

				int check = 0;

				for (int s=0;s<sc->size();s++)
				{
					Stiff* sm = sc->at(s);


					if (sm->index[0]==vert->index&&sm->index[1]==neigh->index)
					{
						sm->stiff += K;

						check++;

					}
				}

				if (check!=1)
				{
					printf("wrong\n");
				}



			}
		}



	}

	
	
}